#include "WProgram.h"
#include <ArCan.h>
#include <LiquidCrystal.h>
#include <SPI.h>

LiquidCrystal lcd(1, 0, 16, 17, 18, 19);

// Variable global
tArCan::tCAN ignitionOnMsgCan;  
tArCan::tCAN vehicleSpeedEngineSpeedMsgCan; 

int velocidade = 100; // 100 km/h
int vel_entr = 1;
int vel = 0;

void setup(){

  lcd.begin(16, 2);

  pinMode(ArCan_CS,OUTPUT);
  pinMode(P_SCK,OUTPUT);
  pinMode(P_MOSI,OUTPUT);
  pinMode(P_MISO,INPUT);

  pinMode(ArCan_INT,INPUT);

  SPI.begin();

  //activamos el SPI de Arduino como Master
  SPCR = (1<<SPE)|(1<<MSTR) | (0<<SPR1)|(1<<SPR0);
  SPSR = 0;

  printToLcd(0,0,"Iniciando CAN");
  delay(500);

  // Preparar a mensagem de "ignition on"
  ignitionOnMsgCan.id = 0x201;
  ignitionOnMsgCan.header.rtr = 0;
  ignitionOnMsgCan.header.length = 8;
  ignitionOnMsgCan.data[0]=0x03;
  ignitionOnMsgCan.data[1]=0xE8;
  ignitionOnMsgCan.data[2]=0x00;
  ignitionOnMsgCan.data[3]=0x00;
  ignitionOnMsgCan.data[4]=0x01;
  ignitionOnMsgCan.data[5]=0x2C;
  ignitionOnMsgCan.data[6]=0x00;
  ignitionOnMsgCan.data[7]=0x00;
  tArCan arcan;

  // Gerar a mensagem
  unsigned char myData[8] = {0};
  calculateVS(3.0, 1000.0,myData);

   // Preparar a mensagem de "vehicle Speed"
   vehicleSpeedEngineSpeedMsgCan.id = 0x201;
   vehicleSpeedEngineSpeedMsgCan.header.rtr = 0;
   vehicleSpeedEngineSpeedMsgCan.header.length = 8;
   vehicleSpeedEngineSpeedMsgCan.data[0]=byte(myData[0]);
   vehicleSpeedEngineSpeedMsgCan.data[1]=byte(myData[1]);
   vehicleSpeedEngineSpeedMsgCan.data[2]=byte(myData[2]);
   vehicleSpeedEngineSpeedMsgCan.data[3]=byte(myData[3]);
   vehicleSpeedEngineSpeedMsgCan.data[4]=byte(myData[4]);
   vehicleSpeedEngineSpeedMsgCan.data[5]=byte(myData[5]);
   vehicleSpeedEngineSpeedMsgCan.data[6]=byte(myData[6]);
   vehicleSpeedEngineSpeedMsgCan.data[7]=byte(myData[7]);
   
  printToLcd(0,0,"Iniciando arcan...");
  delay(1000);

  boolean arcanInitFlag = arcan.init();

  printToLcd(0,0,"Comando arcan.init executado...");
  delay(2000);

  if(arcanInitFlag){ 
    printToLcd(0,0,"MCP iniciou",false);
    printToLcd(0,1,"Corretamente",false);
  }
  else {
    printToLcd(0,0,"MCP NAO iniciou",false);
    printToLcd(0,1,"Corretamente",false);
  }
  delay(3000);

  arcan.mode_normal();
  
  do{
    arcan.send_message(&ignitionOnMsgCan);
    arcan.send_message(&vehicleSpeedEngineSpeedMsgCan);
  } while(1);
  
}

void loop(){
}

void printToLcd(int row, int col, String text){
  printToLcd(row,col,text,true);
}

void printToLcd(int row, int col, String text, boolean clear){
  if(clear == true){
    lcd.clear(); 
  }
  lcd.setCursor(row, col);
  lcd.print(text);
}

/*
 * Recebe a velocidade do carro (dVehicleSpeed) e do motor (dEngineSpeed),
 * retorna o dado da mensagem a ser enviada manipulando um array (do tipo unsigned char Data[8]) 
 */
void calculateVS ( double dVehicleSpeed, double dEngineSpeed, unsigned char *Data)
{
  int iVsTemp,iEsTemp;
  unsigned char chTemp[4]={0};
  //unsigned char Data[8]={0};
  /*--------------------------------------------------------------------------------------------------------------
   Calcula el Engine Speed a enviar
   ----------------------------------------------------------------------------------------------------------------*/
  iEsTemp = (int)( dEngineSpeed );
  // Bit0 = Speed Status
  // Bit1-Bit15 = Speed Value
  iEsTemp = iEsTemp & 0xEFFF; 		// Ponemos la mascara del Valor 
  ///iEsTemp = iEsTemp * 2; 			// Preparamos los Bytes a Enviar   
  /*--------------------------------------------------------------------------------------------------------------
   Calcula el Vehicle Speed a enviar
   ----------------------------------------------------------------------------------------------------------------*/
  iVsTemp = (int)( dVehicleSpeed * 100.0 );
  // Bit0 = Speed Status
  // Bit1-Bit15 = Speed Value
  iVsTemp = iVsTemp & 0xEFFF; 		// Ponemos la Mascara adecuada 
  //iVsTemp = iVsTemp * 2; 				// Preparamos los Bytes a Enviar  
  /*--------------------------------------------------------------------------------------------------------------
   Separar el Vehicle speed en dos bytes
   ----------------------------------------------------------------------------------------------------------------*/
  iVsTemp = iEsTemp + ( iVsTemp<<16 );
  *((int*)(chTemp)) = *((int*)( &iVsTemp )); //Separate in Four bytes
  /*--------------------------------------------------------------------------------------------------------------
   Montar la trama de datos a enviar
   ----------------------------------------------------------------------------------------------------------------*/
  // a mensagem id do speed deve ser 0x201
  Data[0]= chTemp[1];
  Data[1]= chTemp[0];
  Data[2]= 0;
  Data[3]= 0;
  Data[4]= chTemp[3]; // MSB
  Data[5]= chTemp[2]; // LSB
  Data[6]= 0; 
  Data[7]= 0;
  
}




